
#include "stm32f4xx.h"
#include "bsp.h"
#include "icm40607.h"



/**
  * @name				SPI1_ReadWriteByte
  * @brief
  * @param  		send data
  * @retval 		receive data
  */
u8 SPI1_ReadWriteByte(u8 Txdata)
{
	u8 retry = 0;
	while(SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_TXE	) == RESET)
	{
		retry ++;
		if(retry >200)
			return FALSE;
	}
	SPI_I2S_SendData(SPI1,Txdata);
	retry = FALSE;
	
	while(SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_RXNE	) == RESET)
	{
		retry ++;
		if(retry >200)
			return FALSE;
	}
	return SPI_I2S_ReceiveData(SPI1);
}

/**
  * @name				IMU_Read_Reg
  * @brief			read one regisiter from icm40607 
  * @param  		IMU register address
  * @retval 		data of IMU register
  */
u8 IMU_Read_Reg( u8 reg)
{
	u8 rea_val;
	IMU_CS_ENABLE;
	SPI1_ReadWriteByte(reg | 0x80);//reg address+read command
	rea_val = SPI1_ReadWriteByte(0xff);
	IMU_CS_DISABLE;
	return (rea_val) ;

}

u8 IMU_Write_Reg(u8 reg, u8 value)
{
	u8 status;
	IMU_CS_ENABLE;
	status = SPI1_ReadWriteByte(reg);
	SPI1_ReadWriteByte(value);
	IMU_CS_DISABLE;
	return status;
	
}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void IMU_ReadBytes(u8 *pBuff, u8 ReadAddr, u8 NumbyteToRead)
{
	u8 i;
	IMU_CS_ENABLE;
	SPI1_ReadWriteByte(ReadAddr | 0x80);//send read address
	for( i=0; i< NumbyteToRead; i++)
	{
		*(pBuff +i) = SPI1_ReadWriteByte(0xff);
	}
	IMU_CS_DISABLE;
}



void ReadIMU_AllSensors(IMU_SensorRawDataTypeDef *pStr)
{
//	IMU_SensorData sensor_res;
	u8 Reg[14] = {0};
	
	IMU_ReadBytes(Reg,TEMP_DATA1,14);
	
	pStr->temp  = (s16)(( Reg[0]<<8 )| Reg[1]);		
	pStr->accel_x = (s16)(( Reg[2]<<8 )| Reg[3]);
	pStr->accel_y = (s16)(( Reg[4]<<8 )| Reg[5]);
	pStr->accel_z = (s16)(( Reg[6]<<8 )| Reg[7]);
	pStr->gyro_x  = (s16)(( Reg[8]<<8 )| Reg[9]);
	pStr->gyro_y  = (s16)(( Reg[10]<<8 )| Reg[11]);
	pStr->gyro_z  = (s16)(( Reg[12]<<8 )| Reg[13]);
	
}
/**
  * @name					ReadIMU_Config
  * @brief				read configaration of icm40607 ,acc scale & gyro scale
  * @param  			IMU_ConfigTypeDef *confg ,storage of configaration
  * @retval 			none
  */
void ReadIMU_Config(IMU_ConfigTypeDef *confg)
{
	u8 reg_data[2] = {0};

	IMU_ReadBytes(reg_data, GYRO_CONFIG0, 2);
	
	switch((reg_data[1] & ACCEL_FS_MASK) >> 5){
		case 0:
		default:
			confg->accel_LSB = ACCEL_LSB_16G;
		break;
		case 1:
			confg->accel_LSB = ACCEL_LSB_8G;
		break;
		case 2:
			confg->accel_LSB = ACCEL_LSB_4G;
		break;
		case 3:
			confg->accel_LSB = ACCEL_LSB_2G;
		break;
	}

	switch((reg_data[0] & GYRO_FS_MASK) >> 5){
		case 0:
		default:
			confg->gyro_LSB = (float)(GYRO_LSB_2000);
		break;
		case 1:
			confg->gyro_LSB = (float)(GYRO_LSB_1000);
		break;
		case 2:
			confg->gyro_LSB = (float)(GYRO_LSB_500);
		break;
		case 3:
			confg->gyro_LSB = (float)(GYRO_LSB_250);
		break;
		case 4:
			confg->gyro_LSB = (float)(GYRO_LSB_125);
		break;
		case 5:
			confg->gyro_LSB = (float)(GYRO_LSB_62_5);
		break;
		case 6:
			confg->gyro_LSB = (float)(GYRO_LSB_31_25);
		break;
		case 7:
			confg->gyro_LSB = (float)(GYRO_LSB_15_625);
		break;
	}

}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SPI_DMASendStart(void)
{
//	DMA_SetCurrDataCounter(DMA_STREAM_SPI1_TX,bufsize);
//	DMA_SetCurrDataCounter(DMA_STREAM_SPI1_RX,bufsize);
	SPI1->DR = 0;

	DMA_ClearFlag(DMA_STREAM_SPI1_TX,DMA_IT_TCIF3);
	DMA_ClearFlag(DMA_STREAM_SPI1_RX,DMA_IT_TCIF2);
	
	IMU_CS_ENABLE;
	DMA_Cmd(DMA_STREAM_SPI1_TX,ENABLE);
	DMA_Cmd(DMA_STREAM_SPI1_RX,ENABLE);
}
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SPI_DMASendStop(void)
{
	DMA_Cmd(DMA_STREAM_SPI1_TX,DISABLE);
	DMA_Cmd(DMA_STREAM_SPI1_RX,DISABLE);
	
	
	IMU_CS_DISABLE;	
}


/******************************* END OF FILE of icm40607 ****************************/
	




